#include "Spline.hpp"

//  Splines
// ==================================================================
// Cada polinomio de grado 3 es de la forma:
//     S_i(x) = a_i + b_i*(x-x_j) + c_i*(x-x_j)^2 + d_i*(x-x_j)^3

Spline::Spline( const std::vector<Point>& control_points ) :
	_cp( control_points )
{
	_a = new double[ control_points.size() ];
	_b = new double[ control_points.size() ];
	_c = new double[ control_points.size() ];
	_d = new double[ control_points.size() ];

	calc();
}

void Spline::calc()
{
	// TODO: ver si se puede optimizar y hacer con matrices
	uint n = _cp.size() - 1;

	// inicializa a_i = f(x_i)
	uint p = 0;
	foreach( point, _cp )
		_a[ p++ ] = point->y;

	double h[ _cp.size() ];
	// h_i = x_i+1 - x_i	
	forn(i,n-1)
		h[i] = _cp[i+1].x - _cp[i].x;

	double alpha[ _cp.size() ];
	// alpha_i = 3/h_i * (a_i+1 - a_i) - 3/h_(i-1) * (a_i - a_i-1)
	forsn(i,1,n-1)
		alpha[i] = 3.0f/h[i] * (_a[i+1]-_a[i]) - 3.0f/(h[i-1])*(_a[i]-_a[i-1]);

	double l[ _cp.size() ];
	double m[ _cp.size() ];
	double z[ _cp.size() ];

	l[0] = 1;
	m[0] = 0;
	z[0] = 0;

	forsn(i,1,n-1)
	{
		l[i] = 2*(_cp[i+1].x - _cp[i-1].x) - h[i-1]*m[i-1];
		m[i] = h[i]/l[i];
		z[i] = (alpha[i] - h[i-1]*z[i-1])/l[i];
	}

	l[n] = 1;
	z[n] = 0;
	_c[n] = 0;

	for( int j = n-1 ; j >= 0 ; j-- )
	{
		_c[j] = z[j] - m[j] * _c[j+1];
		_b[j] = (_a[j+1]-_a[j])/h[j] - h[j]*(_c[j+1]+2*_c[j])/3.0f;
		_d[j] = (_c[j+1]-_c[j])/(3*h[j]);
	}
}

Spline::~Spline()
{
	delete[] _a;
	delete[] _b;
	delete[] _c;
	delete[] _d;
}

uint Spline::findPol( double x ) const
{
	uint i = 0;
	while( _cp[i+1].x < x && i+1 < _cp.size() ) i++;
	return i;
}

double Spline::derivative( double x ) const
{
	return derivative( x, findPol(x) );
}

double Spline::derivative2( double x ) const
{
	return derivative2( x, findPol(x) );
}

double Spline::eval( double x, uint i ) const
{
	double h = (x-_cp[i].x);
	return _a[i] + _b[i]*h + _c[i]*h*h + _d[i]*h*h*h;
}

double Spline::derivative( double x, uint i ) const
{
	double h = (x-_cp[i].x);
	return 3*_d[i]*h*h + 2*_c[i]*h + _b[i];
}

double Spline::derivative2( double x, uint i ) const
{
	double h = (x-_cp[i].x);
	return 6*_d[i]*h + 2*_c[i];
}

double Spline::operator() ( double x ) const
{
	return eval( x, findPol( x ) );
}

std::ostream& Spline::polynomial( std::ostream& out, uint k, char var ) const
{	
	double xk = _cp[k].x;
	out << _d[k] << "(" << var << " - " << xk << ")³ + "
	    << _c[k] << "(" << var << " - " << xk << ")² + "  
	    << _b[k] << "(" << var << " - " << xk << ") + "
		<< _a[k];
	return out;
}

std::vector<Spline::Point> Spline::controlPoints() const
{
	return _cp;
}

std::ostream& operator<<( std::ostream& out, const Spline& s )
{
	forn( k, s.controlPoints().size()-2 )
	{
		s.polynomial( out, k, 'x' );
		out << std::endl;
	}
	return out;
}

